{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2020-05-08T22:05:05.405241Z",
     "start_time": "2020-05-08T22:05:05.397090Z"
    }
   },
   "source": [
    "\n",
    "<div style=\"border:1px solid black; padding:20px 20px;text-align: justify;text-justify: inter-word\">\n",
    "    <strong>Exercise Session 5 - Path Planning<br/> Duration : 4 hours (2 in session + 2 at home)</strong><br/><br/>\n",
    "    <span style=\"text-decoration:underline;font-weight:bold;\">How to use this notebook?</span><br/>\n",
    "    This notebook is made of text cells and code cells. The code cells have to be <strong>executed</strong> to see the result of the program. To execute a cell, simply select it and click on the \"play\" button (<span style=\"font: bold 12px/30px Arial, serif;\">&#9658;</span>) in the tool bar just above the notebook, or type <code>shift + enter</code>. It is important to execute the code cells in their order of appearance in the notebook.<br/>\n",
    "You can make use of the table of contents to navigate easily between sections.\n",
    "</div>\n",
    "\n",
    "<br/>\n",
    "\n",
    "<div style=\"justify;text-justify: inter-word\">\n",
    "So that you may familiarise with the notebooks and the basic python syntax, the exercises are provided in notebook form and whenever there are any calculations to be made, we encourage you to do them by code. Also, if you want to take notes, we encourage you to use the markdown or Raw NBConvert cells. \n",
    "</div>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "toc": true
   },
   "source": [
    "# Table of Contents\n",
    "\n",
    "1. [Learning Goals](#learning-goals)\n",
    "2. [Understanding Path Planning on Paper](#understanding-path-planning-on-paper)\n",
    "   1. [Dijkstra's Algorithm](#dijkstras-algorithm)\n",
    "   2. [A*](#a-algorithm)\n",
    "3. [Implementing the Dijkstra and A* Algorithm](#implementing-the-dijkstra-and-a-algorithm)\n",
    "   1. [Dijkstra with constant weights](#dijkstra-with-constant-weights)\n",
    "   2. [Dijkstra](#dijkstra)\n",
    "   3. [A*](#a)\n",
    "5. [Please come back to the questionnaire (survey)](https://forms.gle/7Q6z1Q6Y5Q6Q6Q6z6)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "---\n",
    "\n",
    "# Learning Goals\n",
    "\n",
    "- Understand path planning algorithms conceptually and on paper.\n",
    "- Implement and compare path planning algorithms (A* and Dijkstra) in terms of efficiency and performance.\n",
    "\n",
    "# Requirements\n",
    "\n",
    "- The printed ``grid1.pdf`` on paper. \n",
    "\n",
    "<img src=\"Images/grid1.png\" alt=\"gradient\" width=\"300\"/>\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "from matplotlib.colors import ListedColormap\n",
    "from heapq import heappush, heappop # used if you want to use heap queue for priority queue (easier to implement) but not compulsory"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Understanding Path Planning on **Paper**\n",
    "\n",
    "For the paper based algorithms, you will need the files located in the Paper folder : \n",
    "\n",
    "- ``grid1.pdf` for the Dijkstra and A* algorithms on paper. \n",
    "\n",
    "\n",
    "## Dijkstra's Algorithm\n",
    "\n",
    "Apply the Dijkstra’s algorithm on **paper** on the map of the Figure below using a Manhattan distance.\n",
    "\n",
    "<img src=\"Images/grid1.png\" alt=\"gradient\" width=\"300\"/>\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Understanding Path Planning on Paper\n",
    "\n",
    "For the paper based algorithms, you will need the files located in the Paper folder : \n",
    "\n",
    "- ``grid1.pdf`` or `grid-path-planning.xlsx` for the Dijkstra and A* algorithms depending on whether you prefer working on a spreadsheet or on paper. \n",
    "\n",
    "\n",
    "## Introduction to path planning\n",
    "\n",
    "Once a robot has a map, it is essential to be able to plan a path to go from one point to another. The techniques used to find the best path depend on the type of map and of the type of path that one wants to obtain. In many cases, the maps are transformed into graphs and the path search is transformed in a graph search.\n",
    "\n",
    "\n",
    "## Dijkstra's Algorithm\n",
    "\n",
    "Apply the Dijkstra’s algorithm on paper on the map of the Figure below using a Manhattan distance.\n",
    "\n",
    "\n",
    "<img src=\"Images/grid1.png\" alt=\"gradient\" width=\"300\"/>\n",
    "\n",
    "<span style=\"color: #2980B9 ;\">\n",
    "\n",
    "> The solution is provided in the image below. <br>\n",
    "<img src=\"Images/grid-plan.png\" alt=\"gradient\" width=\"600\"/>\n",
    "\n",
    "(a) After five iterations of Dijkstra’s algorithm. (b) The final grid map with the shortest path marked\n",
    "<br>\n",
    "<br>\n",
    "> The solution for the <strong> (extra)</strong> exercise is provided in the image below. <br>\n",
    "<img src=\"Images/grid-plan_2.png\" alt=\"gradient\" width=\"600\"/>\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## A* Algorithm\n",
    "\n",
    "Apply the A* algorithm on paper on the map of the previous exercise using a Manhattan distance from the source for the motion cost and the Manhattan distance to the goal (without obstacles) as heuristic function. The same excel file as for the previous exercice can be used here.\n",
    "\n",
    "<span style=\"color: #2980B9 ;\">\n",
    "\n",
    "> The solution for the <strong> extra </strong> exercise is given in the image below, presenting both the cost function and the heuristic function and their sum. The A* algorithm allows to reduce the number of evaluated cell, at a cost of a higher computation for each cell. If the heuristic function is simple the A* algorithm is more efficient than Dijkstra’s algorithm. <br><br>\n",
    "> a) Heuristic function. b) The first two iterations of the A algorithm\n",
    "\n",
    "\n",
    "<img src=\"images/astar_corr.png\" alt=\"gg\" width=\"600\"/>\n",
    "\n",
    "> c) The A* algorithm after 6 steps. d) The A* algorithm reaches the goal cell and finds a shortest path\n",
    "\n",
    "<img src=\"images/astar-corr_1.png\" alt=\"gg\" width=\"600\"/>\n",
    "\n",
    "\n",
    "<br><br>\n",
    "\n",
    "> The solution for the <strong> extra </strong> exercise is given in the image below, presenting both the cost function and the heuristic function, their sum and the comparison with the result of Dijkstra’s algorithm. The A* algorithm allows to reduce the number of evaluated cell, at a cost of a higher computation for each cell. If the heuristic function is simple, like in this case, the A* algorithm is more efficient than Dijkstra’s algorithm. <br><br>\n",
    "![Grid1](Images/astar-corr_2.png)\n",
    "<br>a) Iterative Manhattan distance to start (function f), <br><br>b) Manhattan distance to goal without considering obstacles (function h), <br><br>c) sum of both functions and minimal computation zone, <br><br>d) comparison with the previous result."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Implementing the Dikstra Algorithm with constant weights (~ Breadth-First Search [BFS])\n",
    "\n",
    "By this point, you should have a better idea regarding how the Dijstra algorithm works. The objective is to compute and display the path as well as the cells that were visited during the search process. \n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "> We recommend starting with the 4-connected grid (Manhattan distance) and **not** the 8-connected grid, which includes diagonals, for now.\n",
    "\n",
    "<img src=\"Images/Square_4_connectivity-1.png\" alt=\"gradient\" width=\"150\"/>\n",
    "<img src=\"Images/Square_8_connectivity.png\" alt=\"gradient\" width=\"150\"/>\n",
    "\n",
    "\n",
    "---"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here is the pseudocode for the Dijkstra algorithm (Basic ~ Breaddth-First Search) : \n",
    "\n",
    "<img src=\"Images/pseudo.png\" alt=\"gradient\" width=\"450\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "\" pseudocode for Dijkstra's algorithm in text form\\nVariables:\\n\\ninteger n ← 0                  # Distance from start\\ncell array grid ← all unmarked # Grid map\\ncell list path ← empty         # Shortest path\\ncell current                   # Current cell in path\\ncell c                         # Index over cells\\ncell S ← ...                   # Source cell\\ncell G ← ...                   # Goal cell\\n\\nmark S with n\\nwhile G is unmarked\\n    n ← n + 1\\n    for each unmarked neighbor (c) of the current frontier cells explored.\\n        mark c with n\\n\\ncurrent ← G\\nappend current to path\\nwhile S not in path\\n    append lowest marked neighbor c of current to path\\n    current ← c\\n\""
      ]
     },
     "execution_count": 10,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "\"\"\" pseudocode for Dijkstra's algorithm in text form\n",
    "Variables:\n",
    "\n",
    "integer n ← 0                  # Distance from start\n",
    "cell array grid ← all unmarked # Grid map\n",
    "cell list path ← empty         # Shortest path\n",
    "cell current                   # Current cell in path\n",
    "cell c                         # Index over cells\n",
    "cell S ← ...                   # Source cell\n",
    "cell G ← ...                   # Goal cell\n",
    "\n",
    "mark S with n\n",
    "while G is unmarked\n",
    "    n ← n + 1\n",
    "    for each unmarked neighbor (c) of the current frontier cells explored.\n",
    "        mark c with n\n",
    "\n",
    "current ← G\n",
    "append current to path\n",
    "while S not in path\n",
    "    append lowest marked neighbor c of current to path\n",
    "    current ← c\n",
    "\"\"\""
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Map \n",
    "\n",
    "We first choose an environment for the grid map with <span style=\"color:green\"><strong>S  </strong></span> (Source/Start in green), <span style=\"color:red\">  <strong>G  </strong></span> (Goal in red), and <span style=\"color:black\"><strong>obstacles  </strong></span> (in black), as shown below.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"Images/initial_map.png\" alt=\"gradient\" width=\"300\"/>\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": []
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Implementation the Dijkstra algorithm with constant weights (BFS)\n",
    "Now you can implement the Dijkstra algorithm with constant weights (BFS) using either the pseudo-code provided and following the structure below or selecting another one."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "> **!! You are free to use any structure you want to implement the algorithms !!**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Path found:\n",
      "59 operations\n"
     ]
    },
    {
     "data": {
      "image/png": "[encoded data, 8492 characters]",
      "text/plain": [
       "<Figure size 432x504 with 1 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "from matplotlib.colors import ListedColormap\n",
    "\n",
    "\n",
    "def display_map(map_grid, path, start, goal, grid):\n",
    "    cmap = ListedColormap(['white', 'black', 'blue', 'green', 'red', 'grey'])\n",
    "    map_display = np.zeros_like(map_grid, dtype=object)\n",
    "# Assign colors based on the map grid values\n",
    "    map_display[map_grid == -1] = 'black'  # Obstacles\n",
    "    map_display[map_grid == 0] = 'white'   # Free space\n",
    "\n",
    "    # Visualize the explored cells (cells that have been marked)\n",
    "    explored_positions = np.argwhere(grid >= 0)\n",
    "    for position in explored_positions:\n",
    "        if map_display[tuple(position)] == 'white':\n",
    "            map_display[tuple(position)] = 'grey'  # Explored cells\n",
    "\n",
    "    # Visualize the path\n",
    "    for position in path:\n",
    "        if map_display[position[0], position[1]] in ['white', 'grey']:\n",
    "            map_display[position[0], position[1]] = 'blue'  # Path\n",
    "\n",
    "    # Set start and goal colors\n",
    "    map_display[start[0], start[1]] = 'green'  # Start\n",
    "    map_display[goal[0], goal[1]] = 'red'      # Goal\n",
    "\n",
    "    # Convert color names to numbers for plotting\n",
    "    color_mapping = {'white': 0, 'black': 1, 'blue': 2, 'green': 3, 'red': 4, 'grey': 5}\n",
    "    map_numeric_display = np.vectorize(color_mapping.get)(map_display)\n",
    "\n",
    "    fig, ax = plt.subplots(figsize=(6, 7))\n",
    "    ax.imshow(map_numeric_display, cmap=cmap)\n",
    "\n",
    "    # Set ticks to show grid lines\n",
    "    ax.set_xticks(np.arange(-0.5, map_grid.shape[1], 1), minor=True)\n",
    "    ax.set_yticks(np.arange(-0.5, map_grid.shape[0], 1), minor=True)\n",
    "    ax.grid(which='minor', color='gray', linestyle='-', linewidth=0.5)\n",
    "    ax.tick_params(which='both', bottom=False, left=False, labelbottom=False, labelleft=False)\n",
    "    ax.set_title('dijkstra_search_with_constant_weight (~BFS) Visualization')\n",
    "\n",
    "    plt.show()\n",
    "\n",
    "def dijkstra_search_with_constant_weight(map_grid, start, goal):\n",
    "    # Initialize variables\n",
    "    n = 0  # Distance from start\n",
    "    grid = np.full_like(map_grid, -2)  # Initialize grid with -2 (unmarked)\n",
    "    path = []  # Shortest path\n",
    "    current = None  # Current cell in path\n",
    "\n",
    "    # Mark the start cell with n\n",
    "    grid[start] = n\n",
    "\n",
    "    # Initialize a list to keep track of the frontier cells\n",
    "    frontier = [start]\n",
    "\n",
    "    operation_count = 0\n",
    "\n",
    "    while grid[goal] == -2:\n",
    "        operation_count += 1\n",
    "        n += 1\n",
    "        new_frontier = []\n",
    "        # For each cell in the current frontier\n",
    "        for cell in frontier:\n",
    "            # Get neighbors of the cell (up, down, left, right)\n",
    "            neighbors = [\n",
    "                (cell[0]-1, cell[1]),  # Up\n",
    "                (cell[0]+1, cell[1]),  # Down\n",
    "                (cell[0], cell[1]-1),  # Left\n",
    "                (cell[0], cell[1]+1)   # Right\n",
    "            ]\n",
    "            for neighbor in neighbors:\n",
    "                # Check if neighbor is within bounds and \n",
    "                if (0 <= neighbor[0] < grid.shape[0]) and (0 <= neighbor[1] < grid.shape[1]):# Check if neighbor is within bounds\n",
    "                    if grid[neighbor] == -2: # Unmarked cell  \n",
    "                        if map_grid[neighbor] != -1: # Not an obstacle\n",
    "                            # Mark neighbor with n\n",
    "                            grid[neighbor] = n\n",
    "                            # Add neighbor to new frontier\n",
    "                            new_frontier.append(neighbor)\n",
    "                            operation_count += 1\n",
    "        if not new_frontier:\n",
    "            # No more cells to explore, and goal is unreachable\n",
    "            return None, grid, operation_count\n",
    "        frontier = new_frontier\n",
    "\n",
    "    # Reconstruct the path\n",
    "    current = goal\n",
    "    path.append(current)\n",
    "    while current != start:\n",
    "        neighbors = [\n",
    "            (current[0]-1, current[1]),  # Up\n",
    "            (current[0]+1, current[1]),  # Down\n",
    "            (current[0], current[1]-1),  # Left\n",
    "            (current[0], current[1]+1)   # Right\n",
    "        ]\n",
    "        # Find the neighbor with the lowest mark (n value)\n",
    "        min_n = grid[current]\n",
    "        next_cell = None\n",
    "        for neighbor in neighbors:\n",
    "            if (0 <= neighbor[0] < grid.shape[0]) and (0 <= neighbor[1] < grid.shape[1]): \n",
    "                if grid[neighbor] >=0 and grid[neighbor] < min_n:\n",
    "                    min_n = grid[neighbor]\n",
    "                    next_cell = neighbor\n",
    "                    \n",
    "        if next_cell is None:\n",
    "            # No path found\n",
    "            return None, grid,operation_count\n",
    "        # Append next_cell to path\n",
    "        path.append(next_cell)\n",
    "        current = next_cell\n",
    "    # Reverse the path\n",
    "    path.reverse()\n",
    "    return path, grid,operation_count\n",
    "\n",
    "# Initialize map\n",
    "Map = np.zeros((11, 9))\n",
    "Map[0, :] = -1; Map[10, :] = -1; Map[:, 0] = -1; Map[:, 8] = -1\n",
    "Map[8, 1] = -1; Map[9, 1] = -1; Map[9, 2] = -1; Map[4:6, 4:8] = -1\n",
    "\n",
    "# Initialize search start and goal locations\n",
    "SearchStart = (8, 5)\n",
    "SearchGoal  = (2, 6)\n",
    "\n",
    "# Perform Dijkstra's search\n",
    "path, grid,operation_count = dijkstra_search_with_constant_weight(Map, SearchStart, SearchGoal)\n",
    "\n",
    "# Display the result\n",
    "if path:\n",
    "    print(\"Path found:\")\n",
    "    print(operation_count, \"operations\")\n",
    "    display_map(Map, path, SearchStart, SearchGoal, grid)\n",
    "else:\n",
    "    print(\"No path found.\")\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Dijstra Algorithm (efficient implementation version with cost)\n",
    "Perfect ! Now let's implement the algorithm that are posible to change the weights of the cells (in <span style=\"color: yellow\"><strong>yellow</strong></span>\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "> Note: Now you understand the principle of Dijkstra (Basics), you can use a priority queue (heapq) with heappush and heappop to efficiently select the node with the lowest cost etc..."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"Images/initial_map_with_cost.png\" alt=\"gradient\" width=\"400\"/>\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Path found:\n",
      "Number of operations: 49\n"
     ]
    },
    {
     "data": {
      "image/png": "[encoded data, 6516 characters]",
      "text/plain": [
       "<Figure size 432x504 with 1 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "def dijkstra_search(map_grid, start, goal):\n",
    "    \n",
    "    ## initialize open set, closed set, and g_costs\n",
    "    open_set = []\n",
    "    heappush(open_set, (0, start))  # (g_cost, position)\n",
    "\n",
    "    came_from = {}\n",
    "    g_costs = {start: 0}\n",
    "    explored = set()\n",
    "    operation_count = 0\n",
    "\n",
    "    while open_set: # G goal is unmarked\n",
    "        current_g_cost, current_pos = heappop(open_set)\n",
    "        explored.add(current_pos)\n",
    "        if current_pos == goal: # if G goal is marked\n",
    "            break\n",
    "        # Get neighbors (up, down, left, right)\n",
    "        neighbors = [\n",
    "            (current_pos[0]-1, current_pos[1]),  # Up\n",
    "            (current_pos[0]+1, current_pos[1]),  # Down\n",
    "            (current_pos[0], current_pos[1]-1),  # Left\n",
    "            (current_pos[0], current_pos[1]+1)   # Right\n",
    "        ]\n",
    "\n",
    "        for neighbor in neighbors: # for each neighbor of marked cells\n",
    "            # Check if neighbor is within bounds\n",
    "            if (0 <= neighbor[0] < map_grid.shape[0]) and (0 <= neighbor[1] < map_grid.shape[1]): #check if the neighbor is less than 1 and greater than 0\n",
    "                # Check if neighbor is not an obstacle\n",
    "                if map_grid[neighbor[0], neighbor[1]] != -1:\n",
    "                    tentative_g_cost = current_g_cost + 1  + map_grid[neighbor[0], neighbor[1]]\n",
    "\n",
    "                    if neighbor not in g_costs or tentative_g_cost < g_costs[neighbor]:\n",
    "                        g_costs[neighbor] = tentative_g_cost\n",
    "                        heappush(open_set, (tentative_g_cost, neighbor))\n",
    "                        came_from[neighbor] = current_pos\n",
    "                        operation_count += 1    \n",
    "    # Reconstruct path\n",
    "    if current_pos == goal:\n",
    "        path = []\n",
    "        while current_pos in came_from:\n",
    "            path.append(current_pos)\n",
    "            current_pos = came_from[current_pos]\n",
    "        path.append(start)\n",
    "        return path[::-1], explored,operation_count  # Return reversed path and explored cells\n",
    "    else:\n",
    "    # If we reach here, no path was found\n",
    "        return None, explored\n",
    "\n",
    "##############################################\n",
    "#do not modify this code\n",
    "##############################################\n",
    "def display_map(map_grid, path, start, goal, explored):\n",
    "    cmap = ListedColormap(['white', 'black', 'blue', 'green', 'red', 'grey', 'yellow'])\n",
    "    map_display = np.zeros_like(map_grid, dtype=object)\n",
    "\n",
    "    # Assign colors based on the map grid values\n",
    "    map_display[map_grid == -1] = 'black'  # Obstacles\n",
    "    map_display[map_grid == 0] = 'white'   # Free space\n",
    "\n",
    "    for position in explored:\n",
    "        if map_display[tuple(position)] == 'white':\n",
    "            map_display[tuple(position)] = 'grey'  # Explored cells\n",
    "\n",
    "    # Visualize the path\n",
    "    for position in path:\n",
    "        if map_display[position[0], position[1]] in ['white', 'grey']:\n",
    "            map_display[position[0], position[1]] = 'blue'  # Path\n",
    "\n",
    "    map_display[5, 3] = 'yellow' # Weighted cell\n",
    "    map_display[start[0], start[1]] = 'green'  # Start\n",
    "    map_display[goal[0], goal[1]] = 'red'      # Goal\n",
    "\n",
    "    # Convert color names to numbers for plotting\n",
    "    color_mapping = {'white': 0, 'black': 1, 'blue': 2, 'green': 3, 'red': 4, 'grey': 5, 'yellow': 6}\n",
    "    map_numeric_display = np.vectorize(color_mapping.get)(map_display)\n",
    "    fig, ax = plt.subplots(figsize=(6, 7))\n",
    "    ax.imshow(map_numeric_display, cmap=cmap)\n",
    "    ax.set_xticks(np.arange(-0.5, map_grid.shape[1], 1), minor=True)\n",
    "    ax.set_yticks(np.arange(-0.5, map_grid.shape[0], 1), minor=True)\n",
    "    ax.grid(which='minor', color='gray', linestyle='-', linewidth=0.5)\n",
    "    ax.tick_params(which='both', bottom=False, left=False, labelbottom=False, labelleft=False)\n",
    "    ax.set_title('Dijkstra Pathfinding Visualization')\n",
    "    plt.show()\n",
    "\n",
    "Map = np.zeros((11, 9))\n",
    "\n",
    "Map[5, 3] = 1; # Weighted cell\n",
    "\n",
    "Map[0, :] = -1; Map[10, :] = -1; Map[:, 0] = -1; Map[:, 8] = -1\n",
    "Map[8, 1] = -1; Map[9, 1] = -1; Map[9, 2] = -1; Map[4:6, 4:8] = -1\n",
    "SearchStart = (8, 5) \n",
    "SearchGoal  = (2, 6)\n",
    "\n",
    "path, explored,operation_count = dijkstra_search(Map, SearchStart, SearchGoal)\n",
    "\n",
    "# Display the result\n",
    "if path:\n",
    "    print(\"Path found:\")\n",
    "    print(f\"Number of operations: {operation_count}\")\n",
    "    display_map(Map, path, SearchStart, SearchGoal, explored)\n",
    "else:\n",
    "    print(\"No path found.\")\n",
    "##############################################\n",
    "#do not modify this code\n",
    "##############################################"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Implemententing the A* Algorithm\n",
    "In the same way, let's implement the A* algorithm.\n",
    "\n",
    "> Note that the A* algorithm is an extension of the Dijkstra algorithm. The main difference is that the A* algorithm uses a heuristic function to estimate the cost of the path from the current node to the goal. **Reuse** the Dijksta alogrithm above and add the things for A* algorithm!!\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Path found:\n",
      "Number of operations: 40\n"
     ]
    },
    {
     "data": {
      "image/png": "[encoded data, 6748 characters]",
      "text/plain": [
       "<Figure size 432x504 with 1 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "def heuristic(a, b):\n",
    "    # Implement the Manhattan distance heuristic\n",
    "    return abs(a[0] - b[0]) + abs(a[1] - b[1])\n",
    "\n",
    "def a_star_search(map_grid, start, goal):\n",
    "    # Initialize the open set as a priority queue and add the start node\n",
    "    open_set = []\n",
    "    heappush(open_set, (heuristic(start, goal), 0, start))  # (f_cost, g_cost, position)\n",
    "\n",
    "    # Initialize the came_from dictionary\n",
    "    came_from = {}\n",
    "    # Initialize g_costs dictionary with default value of infinity and set g_costs[start] = 0\n",
    "    g_costs = {start: 0}\n",
    "    # Initialize the explored set\n",
    "    explored = set()\n",
    "    operation_count = 0\n",
    "\n",
    "    while open_set:\n",
    "        # Pop the node with the lowest f_cost from the open set\n",
    "        current_f_cost, current_g_cost, current_pos = heappop(open_set)\n",
    "\n",
    "        # Add the current node to the explored set\n",
    "        explored.add(current_pos)\n",
    "\n",
    "        # For directly reconstruct path\n",
    "        if current_pos == goal:\n",
    "            break\n",
    "\n",
    "        # Get the neighbors of the current node (up, down, left, right)\n",
    "        neighbors = [\n",
    "            (current_pos[0] - 1, current_pos[1]),  # Up\n",
    "            (current_pos[0] + 1, current_pos[1]),  # Down\n",
    "            (current_pos[0], current_pos[1] - 1),  # Left\n",
    "            (current_pos[0], current_pos[1] + 1)   # Right\n",
    "        ]\n",
    "\n",
    "        for neighbor in neighbors:\n",
    "            # Check if neighbor is within bounds and not an obstacle\n",
    "            if (0 <= neighbor[0] < map_grid.shape[0]) and (0 <= neighbor[1] < map_grid.shape[1]):\n",
    "                if map_grid[neighbor[0], neighbor[1]] != -1 and neighbor not in explored:\n",
    "                    # Calculate tentative_g_cost\n",
    "                    tentative_g_cost = current_g_cost + 1 + map_grid[neighbor[0], neighbor[1]]\n",
    "\n",
    "                    # If this path to neighbor is better than any previous one\n",
    "                    if neighbor not in g_costs or tentative_g_cost < g_costs[neighbor]:\n",
    "                        # Update came_from, g_costs, and f_cost\n",
    "                        came_from[neighbor] = current_pos\n",
    "                        g_costs[neighbor] = tentative_g_cost\n",
    "                        f_cost = tentative_g_cost + heuristic(neighbor, goal)\n",
    "\n",
    "                        # Add neighbor to open set\n",
    "                        heappush(open_set, (f_cost, tentative_g_cost, neighbor))\n",
    "                        operation_count += 1\n",
    "\n",
    "    # Reconstruct path\n",
    "    if current_pos == goal:\n",
    "        path = []\n",
    "        while current_pos in came_from:\n",
    "            path.append(current_pos)\n",
    "            current_pos = came_from[current_pos]\n",
    "        path.append(start)\n",
    "        return path[::-1], explored,operation_count\n",
    "    else:\n",
    "        # If we reach here, no path was found\n",
    "        return None, explored,operation_count\n",
    "##############################################\n",
    "#do not modify this code\n",
    "##############################################\n",
    "def display_map(map_grid, path, start, goal, explored):\n",
    "    cmap = ListedColormap(['white', 'black', 'blue', 'green', 'red', 'grey', 'yellow'])\n",
    "    map_display = np.zeros_like(map_grid, dtype=object)\n",
    "\n",
    "    # Assign colors based on the map grid values\n",
    "    map_display[map_grid == -1] = 'black'  # Obstacles\n",
    "    map_display[map_grid == 0] = 'white'   # Free space\n",
    "\n",
    "    for position in explored:\n",
    "        if map_display[tuple(position)] == 'white':\n",
    "            map_display[tuple(position)] = 'grey'  # Explored cells\n",
    "\n",
    "    # Visualize the path\n",
    "    for position in path:\n",
    "        if map_display[position[0], position[1]] in ['white', 'grey']:\n",
    "            map_display[position[0], position[1]] = 'blue'  # Path\n",
    "\n",
    "    map_display[5, 3] = 'yellow' # Weighted cell\n",
    "    map_display[start[0], start[1]] = 'green'  # Start\n",
    "    map_display[goal[0], goal[1]] = 'red'      # Goal\n",
    "\n",
    "    # Convert color names to numbers for plotting\n",
    "    color_mapping = {'white': 0, 'black': 1, 'blue': 2, 'green': 3, 'red': 4, 'grey': 5, 'yellow': 6}\n",
    "    map_numeric_display = np.vectorize(color_mapping.get)(map_display)\n",
    "    fig, ax = plt.subplots(figsize=(6, 7))\n",
    "    ax.imshow(map_numeric_display, cmap=cmap)\n",
    "    ax.set_xticks(np.arange(-0.5, map_grid.shape[1], 1), minor=True)\n",
    "    ax.set_yticks(np.arange(-0.5, map_grid.shape[0], 1), minor=True)\n",
    "    ax.grid(which='minor', color='gray', linestyle='-', linewidth=0.5)\n",
    "    ax.tick_params(which='both', bottom=False, left=False, labelbottom=False, labelleft=False)\n",
    "    ax.set_title('a_star_search Visualization')\n",
    "    plt.show()\n",
    "\n",
    "Map = np.zeros((11, 9))\n",
    "\n",
    "Map[5, 3] = 1; # Weighted cell\n",
    "\n",
    "Map[0, :] = -1; Map[10, :] = -1; Map[:, 0] = -1; Map[:, 8] = -1\n",
    "Map[8, 1] = -1; Map[9, 1] = -1; Map[9, 2] = -1; Map[4:6, 4:8] = -1\n",
    "SearchStart = (8, 5) \n",
    "SearchGoal  = (2, 6)\n",
    "\n",
    "path, explored,operation_count = a_star_search(Map, SearchStart, SearchGoal)\n",
    "\n",
    "# Display the result\n",
    "if path:\n",
    "    print(\"Path found:\")\n",
    "    print(f\"Number of operations: {operation_count}\")\n",
    "    display_map(Map, path, SearchStart, SearchGoal, explored)\n",
    "else:\n",
    "    print(\"No path found.\")\n",
    "##############################################\n",
    "#do not modify this code\n",
    "##############################################\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Comparison of Dijkstra's Algorithm and A* Algorithm with Varying Cell Weights\n",
    "\n",
    "Finally, reuse the code from the previous exercises to visualize the paths found by the A* search algorithm, incorporating the following aspects:\n",
    "\n",
    "- Different costs assigned to the cells (highlighted in yellow)\n",
    "- Alternative heuristic functions (such as Euclidean distance) in an 8-connected grid\n",
    "- Comparison of the number of operations and visited cells for each algorithm\n",
    "- ..."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "---"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "hide_input": false,
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.9.12"
  },
  "toc": {
   "base_numbering": 1,
   "nav_menu": {},
   "number_sections": true,
   "sideBar": true,
   "skip_h1_title": false,
   "title_cell": "Table of Contents",
   "title_sidebar": "Contents",
   "toc_cell": true,
   "toc_position": {},
   "toc_section_display": true,
   "toc_window_display": true
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
